#include "binary_image.h"


//灰度值个数
#define GrayScale 256

//白色和黑色的灰度值
#define WHITE_PIXEL 255
#define BLACK_PIXEL 0

uint8_t bin_image[MT9V03X_H][MT9V03X_W]; //二值化图像

// 全工程维护，左右边线和中线
volatile int left_edge_points[120];
volatile int right_edge_points[120];
volatile int center_line_points[120];

//全工程维护，左环状态机
int zuo_huan_state = 0;
//全工程维护，右环状态机
int you_huan_state = 0;
//全工程维护，十字路口状态机
int shi_zi_state = 0;

int preaim_point = 60;//预瞄点行号

int max_long_col = 0; //最长白列列号
//全工程维护，
int track_width[120]; //赛道宽度
float track_slope = 0; //赛道斜率
float track_error = 0; //赛道误差

extern int dis1_measure_flag;
extern int dis2_measure_flag;
extern int dis3_measure_flag;
extern double distance_measure[3];

/**
 * @brief 赛道数据初始化
 * 
 */
void image_init()
{
    for (int i = 0; i < 120; i++)
    {
        left_edge_points[i] = -1;
        right_edge_points[i] = -1;
        center_line_points[i] = -1;
        track_width[i] = -1;
    }
}

/**
 * @brief 优化大津法求阈值
 * @note 内部限定了总钻风图像宽高188*120，若要移植就把宽高作为传参
 * @param image 摄像头采集的灰度图
 * @return uint8_t 
 */
uint8_t otsuThreshold(uint8_t *image)
{   
    //有几个灰度值(0~255)
    int Pixel_Max = 0; //赋值0用于查找最大值
    int Pixel_Min = 255;  //赋值255用于查找最小值
    uint8_t width = 188;
    uint8_t height = 120;
    int pixelCount[GrayScale] = {0}; //每个灰度值的像素个数,灰度值是下标，数组存个数
    float pixelPro[GrayScale] = {0}; //每个灰度值的像素概率

    int pixelSum=width*height/4; //实际上被统计的像素个数，因为下面的遍历步距是2，所以这里要除以4

    uint8_t threshold = 0;            //大津法阈值
    
    uint8_t *data = image;

    uint32_t gray_sum = 0; // 灰度值总和

    //跳跃式遍历整幅图像，统计每个灰度值的像素个数，查找最大和最小灰度值
    for (int i = 0; i < height;i+=2)
    {
        for(int j = 0;j < width;j+=2)
        {
            pixelCount[(int)data[i*width+j]]++;//统计每个灰度值的像素个数
            gray_sum +=(int)data[i*width+j]; //灰度值总和
            if(data[i*width+j]>Pixel_Max) Pixel_Max=data[i*width+j]; //找最大值
            if(data[i*width+j]<Pixel_Min) Pixel_Min=data[i*width+j]; //找最小值

        }
    }

    // 计算每个灰度值的像素概率
    for (int i = Pixel_Min;i<=Pixel_Max;i++)
    {
        pixelPro[i] = (float)pixelCount[i] / pixelSum; 
    }

    float w0 = 0,w1=0,u0temp=0,u1temp=0,u0=0,u1=0,deltaTmp=0,deltaMax=0;

    // 遍历所有灰度值，假设每个灰度值都是阈值
    for (int j = Pixel_Min;j<Pixel_Max;j++) 
    {
        w0 += pixelPro[j]; //背景部分每个灰度值的概率之和，背景部分比例
        u0temp+= j*pixelPro[j]; //背景部分，每个灰度值点的比例*灰度值之和(加权和)

        w1 = 1 - w0; 
        u1temp = (float)gray_sum/pixelSum - u0temp;

        u0 = u0temp / w0; //背景平均灰度
        u1 = u1temp / w1; //赛道平均灰度
        deltaTmp=(float)(w0*w1*(u0-u1)*(u0-u1)); //类间方差

        // 找最大类间方差以及其对应的灰度值
        if(deltaTmp>deltaMax) 
        {
            deltaMax=deltaTmp;
            threshold=j;
        }

        // 类间方差开始变小了，说明已经找到最大值了，可以退出循环
        if(deltaTmp<deltaMax) 
        {
            break;
        }
    }

    return threshold<30?30:threshold; //阈值不能太小，最小30
}


/**
 * @brief 二值化图像
 * @note 上半部分和下半部分的阈值强度不同，需要维护0.875
 */
void turn_to_bin(void)
{
    uint8_t image_threshold=otsuThreshold(&mt9v03x_image[0][0]);
    for (int i = 0;i<MT9V03X_H/2;i++)
    {
        for (int j = 0;j<MT9V03X_W;j++)
        {
            bin_image[i][j] = mt9v03x_image[i][j] > 0.875*image_threshold ? WHITE_PIXEL : BLACK_PIXEL;
        }
    }

    for (int i = MT9V03X_H / 2;i<MT9V03X_H;i++)
    {
        for (int j = 0;j<MT9V03X_W;j++)
        {
            bin_image[i][j] = mt9v03x_image[i][j] > image_threshold ? WHITE_PIXEL : BLACK_PIXEL;
        }
    }
}


/**
 * @brief      找到图像中包含白色像素最多的列（最长白列）。
 * @details    此函数经过优化，通过交换循环顺序来改善内存访问模式，
 *             利用CPU缓存实现更快的处理速度。
 *             它首先使用一个辅助数组来累加每列的白像素数，
 *             然后从该数组中找出最大值对应的列索引。
 *
 * @param      width         图像的宽度。
 * @param      height        图像的高度。
 * @param      binary_data   指向二值化图像数据的二维数组 (height x width)。
 *
 * @return     int           包含最多白色像素的列的索引。
 */
int find_longest_white_column(int width, int height, unsigned char binary_data[height][width])
{
    // 步骤1: 创建一个辅助数组来存储每一列的白像素计数。
    // 在栈上分配，对于典型的嵌入式图像宽度来说是安全的。
    int column_counts[width];
    // 使用 memset 将所有计数器快速初始化为0。
    // 这通常比循环赋值更快。
    memset(column_counts, 0, sizeof(column_counts));
    // 步骤2: 遍历图像，采用缓存友好的行优先顺序。
    // 外循环遍历行
    for (int row = 0; row < height; row++)
    {
        // 内循环遍历列
        for (int col = 0; col < width; col++)
        {
            // 如果像素是白色 (255)，则增加对应列的计数器。
            // 这里的内存访问是连续的 (data[row][0], data[row][1], ...)，
            // CPU可以有效地预取数据。
            if (binary_data[row][col] == 255)
            {
                column_counts[col]++;
            }
        }
    }
    // 步骤3: 遍历一维计数数组，找到值最大的列。
    int max_white_count = -1; // 初始化为-1，确保任何正计数值都会被选中
    int max_white_col = 0;
    for (int col = 0; col < width; col++)
    {
        if (column_counts[col] > max_white_count)
        {
            max_white_count = column_counts[col];
            max_white_col = col;
        }
    }
    return max_white_col;
}

//寻找左中右边线任务

// 定义常量，消除魔术数字，提高可读性
#define INVALID_POINT -1      // 无效点标记
#define BOTTOM_SPECIAL_ROWS 3 // 底部特殊处理的行数
#define JUMP_SEARCH_STEP 4    // 跳跃搜索的步长

/**
 * @brief  设置指定行的赛道点为无效
 * @param  row_index 要设置的行号
 */
static void mark_line_as_invalid(int row_index)
{
    center_line_points[row_index] = INVALID_POINT;
    left_edge_points[row_index] = INVALID_POINT;
    right_edge_points[row_index] = INVALID_POINT;
}

/**
 * @brief  从起始点向左搜索边界 (跳跃+逐点)
 * @param  row_data 当前行的数据指针
 * @param  start_col 搜索起始列
 * @return 左边界的列坐标
 */
static int search_left_edge_jump(const uint8_t *row_data,int start_col)
{
    int edge=start_col;
    while(edge-JUMP_SEARCH_STEP>0&&row_data[edge-JUMP_SEARCH_STEP]==WHITE_PIXEL)
    {
        edge-=JUMP_SEARCH_STEP;
    }

    while(edge>0&&row_data[edge]==WHITE_PIXEL)
    {
        edge--;
    }

    return edge;
}

/**
 * @brief  从起始点向右搜索边界 (跳跃+逐点)
 * @param  row_data 当前行的数据指针
 * @param  start_col 搜索起始列
 * @param  width 图像宽度
 * @return 右边界的列坐标
 */
static int search_right_edge_jump(const unsigned char *row_data, int start_col, int width)
{
    int edge = start_col;
    // 1. 跳跃式搜索
    while (edge + JUMP_SEARCH_STEP < width - 1 && row_data[edge + JUMP_SEARCH_STEP] == WHITE_PIXEL)
    {
        edge += JUMP_SEARCH_STEP;
    }
    // 2. 逐点精确搜索
    while (edge < width - 1 && row_data[edge] == WHITE_PIXEL)
    {
        edge++;
    }
    return edge;
}


/**
 * @brief 找左中右边线，在这更新三个边线数组
 * 
 * @param width 图像宽度
 * @param height 图像高度
 * @param binary_image 二值化图像
 * @param start_cot 起始列坐标
 */
void calculate_center_line(int width, int height, uint8_t binary_image[height][width], int start_cot)
{
    int current_row = height - 1;
    int left_edge_prev = INVALID_POINT; //上一行的左边界，用以猜测
    int right_edge_prev = INVALID_POINT;    //上一行的右边界，用以猜测
    int lost_line_flag = 0; //丢线标志


    //行数状态机
    while(current_row>=0)
    {
        const uint8_t *row_data = binary_image[current_row]; // 使用指针访问当前行，效率更高
        //一定要把声明放在while开头，刷新
        int left_edge = INVALID_POINT;
        int right_edge = INVALID_POINT;

        if(current_row>=height-BOTTOM_SPECIAL_ROWS) //底部特殊处理
        {   
            //如果起始点不是白色，此行无效
            if(row_data[start_cot]!=WHITE_PIXEL)
            {
                mark_line_as_invalid(current_row);
                current_row--;
                continue;
            }

            left_edge=search_left_edge_jump(row_data,start_cot);
            right_edge=search_right_edge_jump(row_data,start_cot,width);

        }

        else
        {   //丢线了
            if(lost_line_flag)
            {
                mark_line_as_invalid(current_row);
                current_row--;
                continue;
            }

            //搜索左边界
            int search_pos_left= left_edge_prev;

            if(row_data[search_pos_left]==WHITE_PIXEL)//情况1，预测点是白色，向左找黑边
            {
                while(search_pos_left>0&&row_data[search_pos_left]==WHITE_PIXEL)
                {
                    search_pos_left--;
                }

                left_edge = search_pos_left;
                //跳出循环说明要么为黑色，要么就到0，所以判断是不是黑色，是就是当前点，否则就是最左边边界0
            }

            else // 情况2: 预测点是黑色, 向右找白边
            {
                while(search_pos_left<width-1&&row_data[search_pos_left]!=WHITE_PIXEL)
                {
                    search_pos_left++;
                }

                left_edge=row_data[search_pos_left]==WHITE_PIXEL?search_pos_left-1:INVALID_POINT;
                //跳出循环说明要么为白色，要么就到width-1，所以判断是不是白色，是就是当前点，否则就是最右边边界width-1
            }

            if (left_edge == INVALID_POINT)
            {
                lost_line_flag = 1;
            }

            // --- 搜索右边界 (仅在左边界找到时进行) ---
            if (!lost_line_flag)
            {
                int search_pos_right = right_edge_prev;
                if(row_data[search_pos_right]==WHITE_PIXEL)//情况1，预测点是白色，向右找黑边
                {
                    while(search_pos_right<width-1&&row_data[search_pos_right]==WHITE_PIXEL)
                    {
                        search_pos_right++;
                    }

                    right_edge = search_pos_right;
                    //跳出循环说明要么为黑色，要么就到width-1，从列数的角度判断，如果已经找到了最右边还是白色，那就把最右边定为右边界
                }

                else//情况2，预测点是黑色，向左找白边
                {
                    while(search_pos_right>0&&row_data[search_pos_right]!=WHITE_PIXEL)
                    {
                        search_pos_right--;
                    }

                    right_edge=row_data[search_pos_right]==WHITE_PIXEL?search_pos_right+1:INVALID_POINT;

                    //跳出循环说明要么为白色，要么就到0，所以判断是不是白色，是就是当前点，否则就是最左边边界0
                }
            }

            // 如果任何一边搜索失败，则标记丢线
            if (left_edge == INVALID_POINT || right_edge == INVALID_POINT)
            {
                lost_line_flag = 1;
            }
        }

        // --- 更新结果 ---
        if (!lost_line_flag && left_edge < right_edge)
        {
            center_line_points[current_row] = (left_edge + right_edge) / 2;
            left_edge_points[current_row] = left_edge;
            right_edge_points[current_row] = right_edge;
            // 更新下一次迭代的预测值
            left_edge_prev = left_edge;
            right_edge_prev = right_edge;
        }

        else
        {
            mark_line_as_invalid(current_row);
        }

        current_row--;
    }

}

/*--------------—----------数学算法层--------------------------------*/

/**
 * @brief  高效计算方差 (单遍法)
 * @param  data  数据数组指针
 * @param  size  数据个数
 * @return 方差值
 * @note   此算法仅需遍历数据一次，且避免了昂贵的pow()函数。
 */
double calculateVariance(const double *data, int size)
{
    if (size <= 1)
    { // 如果只有一个或零个数据点，方差为0
        return 0.0;
    }

    double sum = 0.0;    // 存储所有数据点的和
    double sum_sq = 0.0; // 存储所有数据点的平方和
    double mean = 0.0;
    double variance = 0.0;

    // --- 单次循环 ---
    for (int i = 0; i < size; i++)
    {
        sum += data[i];
        sum_sq += data[i] * data[i]; // 避免使用pow()
    }

    // --- 根据公式计算 ---
    mean = sum / size;
    // E[X^2] - (E[X])^2
    variance = (sum_sq / size) - (mean * mean);

    return variance;
}



// 预计算常量，避免在函数内重复计算
#define RADIANS_TO_DEGREES (180.0 / 3.1415926f)

/**
 * @brief  高效计算两个向量夹角，单位是度
 * @param  A, B, C 三个点，计算以B为顶点的夹角
 * @return 夹角的余弦值 (范围-1.0到1.0)
 * @note   double angle = calculateAngle(A, B, C);
 */
double calculateAngle(Point A, Point B, Point C)
{
    // 1. 创建向量
    double BA_x = A.x - B.x;
    double BA_y = A.y - B.y;
    double BC_x = C.x - B.x;
    double BC_y = C.y - B.y;
    // 2. 计算点积
    double dot_product = BA_x * BC_x + BA_y * BC_y;
    // 3. 计算模长的平方和 (避免了sqrt)
    double magnitude_sq_BA = BA_x * BA_x + BA_y * BA_y;
    double magnitude_sq_BC = BC_x * BC_x + BC_y * BC_y;
    // 分母的平方
    double denominator_sq = magnitude_sq_BA * magnitude_sq_BC;

    // 检查分母是否为零，防止除零错误
    if (denominator_sq == 0)
    {
        return 1.0; // 向量长度为0，可视为夹角为0，余弦为1
    }
    // 4. 计算余弦值
    double cos_theta = dot_product / sqrt(denominator_sq);
    // 5. 钳位操作 (仍然建议保留，作为浮点误差的最后防线)
    if (cos_theta > 1.0)
        return 1.0;
    if (cos_theta < -1.0)
        return -1.0;
    return acos(cos_theta) * RADIANS_TO_DEGREES;
}


/**
 * @brief  计算拟合直线的斜率 a 和截距 b (优化版)
 * @param  x      数据点的x坐标数组
 * @param  y      数据点的y坐标数组
 * @param  n      数据点的个数
 * @param  a      (输出) 斜率
 * @param  b      (输出) 截距
 * @note   此版本通过以下方式进行了优化：
 *         1. 防止中间乘积溢出。
 *         2. 增加了除零保护，提高了数值稳定性。
 */
void least_squares_fit(const int *x, const int *y, int n, double *a, double *b)
{
    if (n == 0)
    {
        *a = 0;
        *b = 0;
        return;
    }
    double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_x2 = 0.0;
    // 1. 计算各项的和 (优化点：防止溢出)
    for (int i = 0; i < n; i++)
    {
        double xi = (double)x[i];
        double yi = (double)y[i];
        sum_x += xi;
        sum_y += yi;
        sum_xy += xi * yi;
        sum_x2 += xi * xi;
    }
    // 计算分母
    double denominator = (double)n * sum_x2 - sum_x * sum_x;
    // 2. 增加数值稳定性检查 (优化点：防止除零)
    if (fabs(denominator) < 1e-9)
    {
        // 垂直线，斜率无穷大。根据应用场景决定如何处理。
        // 这里我们返回一个极大的斜率值。
        *a = 1e9;
        *b = (sum_y - *a * sum_x) / (double)n; // 此时b的计算可能无意义
        return;
    }

    // 计算斜率和截距
    *a = ((double)n * sum_xy - sum_x * sum_y) / denominator;
    *b = (sum_y - *a * sum_x) / (double)n;
}


//判断左右直道优化版




// --- 1. 宏定义算法参数-----
#define STRAIGHT_START_ROW 3              // 开始检测的行
#define STRAIGHT_SEGMENT_ROWS 10          // 每个线段的行数
#define STRAIGHT_MAX_SEGMENTS 11          // 最多有多少个线段
#define STRAIGHT_VARIANCE_THRESHOLD 0.009 // 斜率方差阈值
#define STRAIGHT_JUMP_THRESHOLD 30        // 边缘跳变阈值

/**
 * @brief  检查图像中是否存在右直道 (参数化尺寸版)
 * @param  width  图像的宽度
 * @param  height 图像的高度
 * @return 如果是直道，返回true；否则返回false。
 * @note   此版本将图像尺寸作为参数传入，提高了函数的通用性。
 */
bool judge_right_stright(int width, int height)
{
    // --- 2. 前置检查：检查右边缘是否完整 ---
    for (int row = STRAIGHT_START_ROW; row < height; row++)
    {
        if (right_edge_points[row] == -1)
        {
            return false; // 边缘不完整，不可能是直道
        }
    }

    // --- 3. 计算所有线段的斜率 ---
    double slopes[STRAIGHT_MAX_SEGMENTS];
    int valid_segment_count = 0;
    for (int i = 0; i < STRAIGHT_MAX_SEGMENTS; i++)
    {
        int start_row = STRAIGHT_START_ROW + i * STRAIGHT_SEGMENT_ROWS;

        // 检查这个线段是否在图像范围内
        if (start_row + STRAIGHT_SEGMENT_ROWS > height)
        {
            break;
        }

        // 准备最小二乘法的数据
        int x_data[STRAIGHT_SEGMENT_ROWS];
        int y_data[STRAIGHT_SEGMENT_ROWS];
        for (int j = 0; j < STRAIGHT_SEGMENT_ROWS; j++)
        {
            x_data[j] = start_row + j;
            y_data[j] = right_edge_points[start_row + j];
        }

        // 计算斜率
        double slope, intercept;
        least_squares_fit(x_data, y_data, STRAIGHT_SEGMENT_ROWS, &slope, &intercept);
        slopes[i] = slope;
        valid_segment_count++;
    }

    if (valid_segment_count == 0)
    {
        return true; // 如果没有有效线段，可以认为没有弯道
    }

    // --- 4. 计算斜率的方差并判断 ---
    double variance = calculateVariance(slopes, valid_segment_count);
    // vriyou = variance; // 如果需要，可以保留这个全局变量用于调试

    if (variance >= STRAIGHT_VARIANCE_THRESHOLD)
    {
        return false; // 方差太大，是弯道
    }

    // --- 5. 检查边缘是否有急剧跳变 ---
    // 从图像底部向上检查
    for (int row = height - 1; row > 0; row--)
    {
        // 检查点是否有效
        if (right_edge_points[row] == -1 || right_edge_points[row - 1] == -1)
        {
            continue;
        }

        // 检查跳变
        if ((right_edge_points[row] - right_edge_points[row - 1] >= STRAIGHT_JUMP_THRESHOLD) ||
            (right_edge_points[row] >= width - 3)) // 接近图像右边缘
        {
            return false;
        }
    }

    return true; // 所有检查通过，是直道
}

/**
 * @brief  检查图像中是否存在左直道 (参数化尺寸版)
 * @param  width  图像的宽度
 * @param  height 图像的高度
 * @return 如果是直道，返回true；否则返回false。
 * @note   此版本将图像尺寸作为参数传入，提高了函数的通用性。
 *         其结构与 judge_right_stright 完全对应。
 */
bool judge_left_stright(int width, int height)
{
    // --- 2. 前置检查：检查左边缘是否完整 ---
    for (int row = STRAIGHT_START_ROW; row < height; row++)
    {
        if (left_edge_points[row] == -1)
        {
            return false; // 边缘不完整，不可能是直道
        }
    }

    // --- 3. 计算所有线段的斜率 ---
    double slopes[STRAIGHT_MAX_SEGMENTS];
    int valid_segment_count = 0;
    for (int i = 0; i < STRAIGHT_MAX_SEGMENTS; i++)
    {
        int start_row = STRAIGHT_START_ROW + i * STRAIGHT_SEGMENT_ROWS;

        // 检查这个线段是否在图像范围内
        if (start_row + STRAIGHT_SEGMENT_ROWS > height)
        {
            break;
        }

        // 准备最小二乘法的数据
        int x_data[STRAIGHT_SEGMENT_ROWS];
        int y_data[STRAIGHT_SEGMENT_ROWS];
        for (int j = 0; j < STRAIGHT_SEGMENT_ROWS; j++)
        {
            x_data[j] = start_row + j;
            y_data[j] = left_edge_points[start_row + j];
        }

        // 计算斜率
        double slope, intercept;
        least_squares_fit(x_data, y_data, STRAIGHT_SEGMENT_ROWS, &slope, &intercept);
        slopes[i] = slope;
        valid_segment_count++;
    }

    if (valid_segment_count == 0)
    {
        return true; // 如果没有有效线段，可以认为没有弯道
    }

    // --- 4. 计算斜率的方差并判断 ---
    double variance = calculateVariance(slopes, valid_segment_count);
    // vrizuo = variance; // 如果需要，可以保留这个全局变量用于调试

    if (variance >= STRAIGHT_VARIANCE_THRESHOLD)
    {
        return false; // 方差太大，是弯道
    }

    // --- 5. 检查边缘是否有急剧跳变 ---
    // 从图像底部向上检查
    for (int row = height - 1; row > 0; row--)
    {
        // 检查点是否有效
        if (left_edge_points[row] == -1 || left_edge_points[row - 1] == -1)
        {
            continue;
        }

        // 检查跳变。注意：左边缘是列坐标减小，所以是 (前一行 - 当前行)
        if ((left_edge_points[row - 1] - left_edge_points[row] >= STRAIGHT_JUMP_THRESHOLD) ||
            (left_edge_points[row] <= 2)) // 接近图像左边缘
        {
            return false;
        }
    }

    return true; // 所有检查通过，是直道
}



/*------------------图像采集后计算误差与对外调用API -----------------------*/


/**
 * @brief 处理左环岛的状态机函数 (规范化命名版)
 * @details
 *  该函数包含一个两状态状态机 (zuo_huan_state):
 *  - 状态0 (STATE_DETECT): 检测进入环岛的条件。
 *  - 状态1 (STATE_IN_ROUNDABOUT): 在环岛内进行路径补偿（补线），并检测出环岛条件。
 *  此函数还会在检测到右侧为直道时，根据历史赛道宽度修正中心线。
 */
void zuo_huan()
{
    // 根据状态控制调试用的GPIO输出
    // 此处为演示，实际应全局替换
    if (zuo_huan_state== 0)
    {
        gpio_set_level(D4, 0);
        gpio_set_level(B9, 0);
    }
    else
    { // left_roundabout_state == 1
        gpio_set_level(D4, 1);
        gpio_set_level(B9, 0);
    }

    // 步骤1: 判断赛道右边缘是否为直道
    // 这是进入环岛的一个关键前置条件
    const int is_right_edge_straight = judge_right_stright(188, 120);

    // 如果右边缘是直道，则根据历史赛道宽度修正中心线
    if (is_right_edge_straight)
    {
        for (int row = 119; row >= 0; row--)
        {
            // 仅在右边缘点有效时进行修正
            if (right_edge_points[row] != -1)
            {
                center_line_points[row] = right_edge_points[row] - track_width[row] / 2;
            }
        }
    }

    // 步骤2: 根据当前状态执行不同的逻辑
    if (zuo_huan_state == 0)
    {
        // === 状态0: 检测进入环岛的条件 ===

        // 条件1: 右边缘线未在远处丢失
        int right_edge_lost_counter = 0;
        int is_right_edge_lost = 0;
        for (int row = 104; row >= 0; row--)
        {
            if (right_edge_points[row] >= 185)
            {
                right_edge_lost_counter++;
                if (right_edge_lost_counter > 10)
                {
                    is_right_edge_lost = 1;
                    break;
                }
            }
        }

        // 条件2: 图像左下角出现内环特征 (左边缘丢失)
        int left_corner_feature_counter = 0;
        for (int row = 119; row >= 50; row--)
        {
            if (left_edge_points[row] <= 2 && left_edge_points[row] != -1)
            {
                left_corner_feature_counter++;
            }
        }
        const int is_left_corner_present = (left_corner_feature_counter >= 30);

        // 组合所有条件进行最终判断
        // 只有在满足所有前置条件时，才进行消耗资源的斜率计算
        if (is_right_edge_straight && is_left_corner_present && !is_right_edge_lost)
        {

            // 条件3: 当前中线斜率与入弯前直道斜率差别不大
            int fit_points_x[25];
            int fit_points_y[25];
            double calculated_slope, calculated_intercept;
            int num_points_for_fit = 0;

            for (int row = 110; row >= 0; row = row - 5)
            {
                if (center_line_points[row] != -1)
                {
                    fit_points_x[num_points_for_fit] = row;
                    fit_points_y[num_points_for_fit] = center_line_points[row];
                    num_points_for_fit++;
                }
            }

            // 确保有足够点进行拟合，防止除零错误
            if (num_points_for_fit > 1)
            {
                least_squares_fit(fit_points_x, fit_points_y, num_points_for_fit, &calculated_slope, &calculated_intercept);

                const double slope_difference = track_slope - calculated_slope;
                const int is_slope_stable = (slope_difference < 0.15) && (slope_difference > -0.15);

                if (is_slope_stable)
                {
                    zuo_huan_state = 1; // 所有条件满足，进入环岛状态
                }
            }
        }
    }
    else
    { // left_roundabout_state == 1
        // === 状态1: 在环岛内，执行路径补偿和出环检测 ===

        // 1. 路径补偿 (补线): 将目标中心线向内侧偏移
        for (int row = 119; row >= 0; row--)
        {
            if (left_edge_points[row] != -1)
            {
                // 目标路径 = 左边缘 + 1/4 历史赛道宽度
                center_line_points[row] = left_edge_points[row] + 0.25 * track_width[row];
            }
        }

        // 2. 出环检测: 基于外部传感器（如超声波或红外距离传感器）
        // 当检测到前方距离超过阈值时，认为已出环
        if ((abs(distance_measure[1]) + abs(distance_measure[2])) / 2 > 44000)
        {
            zuo_huan_state= 0; // 切换回检测状态
            dis2_measure_flag = 1;     // 重置外部模块的标志位
        }
    }
}

/**
 * @brief 处理右环岛的状态机函数 (规范化命名版)
 * @details
 *  该函数包含一个两状态状态机 (you_huan_state):
 *  - 状态0 (STATE_DETECT): 检测进入右环岛的条件。
 *  - 状态1 (STATE_IN_ROUNDABOUT): 在环岛内进行路径补偿（补线），并检测出环岛条件。
 *  此函数还会在检测到左侧为直道时，根据历史赛道宽度修正中心线。
 */
void you_huan()
{
    // 根据状态控制调试用的GPIO输出
    if (you_huan_state == 0)
    {
        gpio_set_level(D4, 0);
        gpio_set_level(B9, 0);
    }
    else
    { 
        gpio_set_level(D4, 1);
        gpio_set_level(B9, 0);
    }

    // 步骤1: 判断赛道左边缘是否为直道
    // 这是进入右环岛的一个关键前置条件
    const int is_left_edge_straight = judge_left_stright(188, 120);

    // 如果左边缘是直道，则根据历史赛道宽度修正中心线
    if (is_left_edge_straight)
    {
        for (int row = 119; row >= 0; row--)
        {
            // 仅在左边缘点有效时进行修正
            if (left_edge_points[row] != -1)
            {
                center_line_points[row] = left_edge_points[row] + track_width[row] / 2;
            }
        }
    }

    // 步骤2: 根据当前状态执行不同的逻辑
    if (you_huan_state == 0)
    {
        // === 状态0: 检测进入环岛的条件 ===

        // 条件1: 左边缘线未在远处丢失
        int left_edge_lost_counter = 0;
        int is_left_edge_lost = 0;
        for (int row = 104; row >= 0; row--)
        {
            if (left_edge_points[row] <= 2 && left_edge_points[row] != -1)
            {
                left_edge_lost_counter++;
                if (left_edge_lost_counter > 10)
                {
                    is_left_edge_lost = 1;
                    break;
                }
            }
        }

        // 条件2: 图像右下角出现内环特征 (右边缘丢失)
        int right_corner_feature_counter = 0;
        for (int row = 119; row >= 50; row--)
        {
            if (right_edge_points[row] >= 185)
            {
                right_corner_feature_counter++;
            }
        }
        const int is_right_corner_present = (right_corner_feature_counter >= 30);

        // 组合所有条件进行最终判断
        // 只有在满足所有前置条件时，才进行消耗资源的斜率计算
        if (is_left_edge_straight && is_right_corner_present && !is_left_edge_lost)
        {

            // 条件3: 当前中线斜率与入弯前直道斜率差别不大
            int fit_points_x[25];
            int fit_points_y[25];
            double calculated_slope, calculated_intercept;
            int num_points_for_fit = 0;

            for (int row = 110; row >= 0; row = row - 5)
            {
                if (center_line_points[row] != -1)
                {
                    fit_points_x[num_points_for_fit] = row;
                    fit_points_y[num_points_for_fit] = center_line_points[row];
                    num_points_for_fit++;
                }
            }

            // 确保有足够点进行拟合，防止除零错误
            if (num_points_for_fit > 1)
            {
                least_squares_fit(fit_points_x, fit_points_y, num_points_for_fit, &calculated_slope, &calculated_intercept);

                const double slope_difference = track_slope - calculated_slope;
                const int is_slope_stable = (slope_difference < 0.15) && (slope_difference > -0.15);

                if (is_slope_stable)
                {
                    you_huan_state = 1; // 所有条件满足，进入环岛状态
                }
            }
        }
    }
    else
    { // right_roundabout_state == 1
        // === 状态1: 在环岛内，执行路径补偿和出环检测 ===

        // 1. 路径补偿 (补线): 将目标中心线向内侧偏移
        for (int row = 119; row >= 0; row--)
        {
            if (right_edge_points[row] != -1)
            {
                // 目标路径 = 右边缘 - 1/4 历史赛道宽度
                center_line_points[row] = right_edge_points[row] - 0.25 * track_width[row];
            }
        }

        // 2. 出环检测: 基于外部传感器
        if ((abs(distance_measure[1]) + abs(distance_measure[2])) / 2 > 44000)
        {
            you_huan_state = 0;         // 切换回检测状态
            dis2_measure_flag = 1;      // 重置外部模块的标志位
        }
    }
}

/**
 * @brief 处理十字路口的状态机函数
 * @details
 *  该函数用于检测并处理十字路口。
 *  1. **检测入口**: 在预瞄点 `preaim_point` 附近检测左右边缘是否同时丢失，若满足则进入十字路口状态 (`shi_zi_state = 1`)。
 *  2. **寻找出口**: 在十字路口状态下，寻找赛道重新出现的左拐点 (`zuo_shi_zi`)或右拐点 (`you_shi_zi`)。
 *  3. **路径修正**: 找到出口后，利用出口前的路径点拟合一条直线，并用该直线重构整个赛道中心线，
 *     以引导车辆稳定地通过路口。
 * @note 原始代码中，仅实现了左拐出口的路径重构逻辑，右拐出口的逻辑为空。此重构严格保留该特性。
 *
 * @example
 * 调用方式:
 *   在计算完左右边缘及初步中线后直接调用 `shi_zi();`
 *
 * 维护的全局变量:
 *   - 读取: `preaim_point`, `right_edge_points`, `left_edge_points`
 *   - 写入: `shi_zi_state`, `center_line_points` (当找到出口时)
 */
void shi_zi()
{
    // === 步骤1: 检测是否进入十字路口 ===
    // 默认先将状态置为0，如果在预瞄区检测到特征再置为1
    shi_zi_state = 0;
    // 遍历预瞄点附近区域 (preaim_point 到 preaim_point + 5)
    for (int row = preaim_point + 5; row >= preaim_point; row--)
    {
        // 条件: 右边缘在最右侧(>=185) 且 左边缘在最左侧(<=2)，即赛道完全开放
        if (right_edge_points[row] >= 185 && left_edge_points[row] <= 2 && left_edge_points[row] != -1)
        {
            shi_zi_state = 1; // 确认进入十字路口状态
            break;            // 找到一个点满足即可退出循环
        }
    }

    // 如果不处于十字路口状态，则无需执行后续逻辑
    if (shi_zi_state == 0)
    {
        return;
    }

    // === 步骤2: 寻找十字路口的出口（左拐或右拐） ===
    int zuo_shi_zi = -1;
    int you_shi_zi = -1;

    // 优先寻找左拐出口
    // 遍历范围从图像顶部到预瞄点后5行
    for (int row = 0; row <= preaim_point + 5; row++)
    {
        // 条件: 左边缘出现向内的大幅度跳变(>20)，且跳变后的点在图像最左侧
        if ((left_edge_points[row] - left_edge_points[row + 1]) > 20 && left_edge_points[row + 1] <= 2 && left_edge_points[row + 1] != -1)
        {
            zuo_shi_zi = row; // 记录下拐点所在的行
            break;
        }
    }

    // 如果没有找到左拐出口，则继续寻找右拐出口
    if (zuo_shi_zi == -1)
    {
        for (int row = 0; row <= preaim_point + 5; row++)
        {
            // 条件: 右边缘出现向内的大幅度跳变(>20)，且跳变后的点在图像最右侧
            if ((right_edge_points[row + 1] - right_edge_points[row]) > 20 && right_edge_points[row + 1] >= 185)
            {
                you_shi_zi = row; // 记录下拐点所在的行
                break;
            }
        }
    }

    // === 步骤3: 根据找到的出口，进行路径外插修正 ===
    if (zuo_shi_zi != -1)
    {
        // --- 处理左拐出口 ---
        const int NUM_POINTS_TO_FIT = 7;
        int fit_points_x[NUM_POINTS_TO_FIT];
        int fit_points_y[NUM_POINTS_TO_FIT];
        double fit_slope, fit_intercept;

        int point_count = 0;
        // 从拐点向前取7个点用于拟合
        for (int row = zuo_shi_zi; row > zuo_shi_zi - NUM_POINTS_TO_FIT; row--)
        {
            // 健壮性检查: 确保行号有效且在数组范围内
            if (row >= 0)
            {
                fit_points_x[point_count] = row;
                fit_points_y[point_count] = center_line_points[row];
                point_count++;
            }
        }

        // 使用这些点拟合出一条直线 (需要至少2个点)
        if (point_count > 1)
        {
            least_squares_fit(fit_points_x, fit_points_y, point_count, &fit_slope, &fit_intercept);

            // 用拟合出的直线方程，重构整个赛道的中线
            for (int row = 119; row >= 0; row--)
            {
                center_line_points[row] = (int)(fit_slope * row + fit_intercept);
            }
        }
    }
    else if (you_shi_zi != -1)
    {
        // --- 处理右拐出口 ---
        // 原始代码在此处为空，严格保持逻辑不变。
    }
}



/**
 * @brief 在main.c中调用
 * 
 */
void bin_image_process()
{
    turn_to_bin();
    //找到最长白列
    max_long_col = find_longest_white_column(MT9V03X_W, MT9V03X_H, bin_image);

    //找左中右边线
    calculate_center_line(MT9V03X_W, MT9V03X_H, bin_image, max_long_col);

    zuo_huan();
    you_huan();
    shi_zi();
        
}